
#include "World.h"

using namespace p2;

void ratchetWheel(real radius, int32 numTeeth, real toothHeight, real motorW)
{
	p2Material material(0.2, 0.2, 0.2);

    ConvexPolygon** t = new ConvexPolygon*[numTeeth];

	Entity* w0 = Entity::create(Vector2<real>(400.f, 300.f), 0.f/*-Math<real>::HALF_PI*/, NULL);
	s_world->attach(w0);

	real s = Math<real>::TWO_PI / (real) numTeeth;
	for (int32 i = 0; i < numTeeth; i++) {
		t[i] = ConvexPolygon::create(20.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
		t[i]->addVertex(0, 0);
		real x1 = radius * Math<real>::Cos((real)i * s);
		real y1 = radius * Math<real>::Sin((real)i * s);
		t[i]->addVertex(x1, y1);
		real x2 = (radius+toothHeight) * Math<real>::Cos((real)(i+1) * s);
		real y2 = (radius+toothHeight) * Math<real>::Sin((real)(i+1) * s);
		t[i]->addVertex(x2, y2);
		w0->attach(t[i]);
	}
	ConstraintAnchor a0(w0, Vector2<real>::ZERO);
	ConstraintAnchor a1(NULL, Vector2<real>(400.f, 300.f));
	PinJoint* p0 = PinJoint::create(a0, a1, 0.f, NULL);
	p0->setFriction(0.01);
	//p0->setLimits(0.5f*Math<real>::PI, 1.5f*Math<real>::PI);
	p0->setSpring(1000.f, 0.f);
	p0->setMotor(motorW, 0);
	s_world->attach(p0);

	Entity* w1 = Entity::create(Vector2<real>(250.f, 425.f), 0.f/*-Math<real>::HALF_PI*/, NULL);
	s_world->attach(w1);
	ConvexPolygon* arm = ConvexPolygon::create(50.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
	arm->makeBox(80, 10);
	w1->attach(arm);
	Disc* pivot = Disc::create(20.f, Vector2<real>(-65, 0), 0.f, material, 0x0000001, NULL);
	pivot->setRadius(20);
	w1->attach(pivot);
	ConstraintAnchor a2(w1, Vector2<real>(-50, 0));
	ConstraintAnchor a3(NULL, Vector2<real>(250.f, 425.f));
	PinJoint* p1 = PinJoint::create(a2, a3, 0.f, NULL);
	p1->setSpring(1000.f, 0.f);
	s_world->attach(p1);

	ConstraintAnchor a4(w0, Vector2<real>::ZERO);
	ConstraintAnchor a5(w1, Vector2<real>(65.f, 0.f));
	PinJoint* p2 = PinJoint::create(a4, a5, 0.f, NULL);
	p2->setSpring(10, 0.01f);
	s_world->attach(p2);

}

